#include "usb.h"
#include "imu.h"
#include "referee_crc.h"
#include "rtdef.h"
#include "rtthread.h"
#include "rttypes.h"
#include <rtdevice.h>

struct Vision_Recv_s Vision_Recv;
struct Vision_Send_s Vision_Send;

rt_device_t dev_vcom = RT_NULL;

#define LOG_TAG              "usb_user"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>


void usb_user_thread_entry(void *parameter)
{
    
    while (1)
    {
        rt_uint8_t tmp[28];
        Vision_Send.header =0X5A;
        Vision_Send.detect_color=0;
        Vision_Send.task_mode=1;
        Vision_Send.reset_tracker=0;
        Vision_Send.is_play=1;
        Vision_Send.change_target=0;
        Vision_Send.reserved =0;
        Vision_Send.roll =INS.Roll;
        Vision_Send.pitch =INS.Pitch;
        Vision_Send.yaw =INS.Yaw;
        Vision_Send.aim_x=0;
        Vision_Send.aim_y=0;
        Vision_Send.aim_z=0;
        Vision_Send.game_time=300;
        Vision_Send.timestamp=rt_tick_get();


        // 复制 header
        tmp[0] = Vision_Send.header;

        // 组合位字段
        uint8_t combined = (Vision_Send.detect_color << 7) | (Vision_Send.reset_tracker << 6)| Vision_Send.reserved;
        tmp[1] = combined;

        // 复制浮点数
        rt_memcpy(tmp + 2, &Vision_Send.roll, sizeof(float));
        rt_memcpy(tmp + 2 + sizeof(float), &Vision_Send.pitch, sizeof(float));
        rt_memcpy(tmp + 2 + 2 * sizeof(float), &Vision_Send.yaw, sizeof(float));
        rt_memcpy(tmp + 2 + 3 * sizeof(float), &Vision_Send.aim_x, sizeof(float));
        rt_memcpy(tmp + 2 + 4 * sizeof(float), &Vision_Send.aim_y, sizeof(float));
        rt_memcpy(tmp + 2 + 5 * sizeof(float), &Vision_Send.aim_z, sizeof(float));
        // 复制 checksum
        Append_CRC16_Check_Sum(tmp, sizeof(tmp));

        //tmp[31] = Vision_Send.checksum >> 8; // 高字节
        //tmp[32] = Vision_Send.checksum & 0xFF; // 低字节

        ros_write(tmp,sizeof(tmp));

        
        rt_thread_mdelay(10);
        

        
    }
}




int usb_task_init(void){
    rt_thread_t thread = rt_thread_create("usb_user", usb_user_thread_entry, RT_NULL, 4096, 30, 10);
    /* 创建成功则启动线程 */
    if (thread != RT_NULL)
    {
        rt_thread_startup(thread);
    }
    else
    {
        return  RT_ERROR;
    }

    LOG_D("usb_user thread init success!\n");
    
    return RT_EOK;

}

//INIT_APP_EXPORT(usb_task_init);

